#include "fsl_common.h"

/*
 * Copyright (c) 2016, Freescale Semiconductor, Inc.
 * Copyright 2016-2020 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */
#include "fsl_flexspi.h"
#include "app.h"
#include "fsl_debug_console.h"
#include "fsl_cache.h"

#include "pin_mux.h"
#include "clock_config.h"
#include "board.h"
#include "fsl_common.h"
/*******************************************************************************
 * Definitions
 ******************************************************************************/

/*******************************************************************************
 * Prototypes
 ******************************************************************************/
extern void flexspi_hyper_flash_init(void);
extern status_t flexspi_nor_flash_erase_sector(FLEXSPI_Type *base, uint32_t address);
extern status_t flexspi_nor_flash_page_program(FLEXSPI_Type *base, uint32_t address, const uint32_t *src);
extern status_t flexspi_nor_hyperflash_cfi(FLEXSPI_Type *base);
extern status_t flexspi_nor_flash_erase_chip(FLEXSPI_Type *base);

/*******************************************************************************
 * Variables
 ******************************************************************************/

static uint8_t s_hyperflash_program_buffer[FLASH_PAGE_SIZE];
static uint8_t s_hyperflash_read_buffer[FLASH_PAGE_SIZE];

/*******************************************************************************
 * Code
 ******************************************************************************/
flexspi_device_config_t deviceconfig = {
    .flexspiRootClk = 196000000, /* 332MHZ SPI serial clock */
    .isSck2Enabled = false,
    .flashSize = FLASH_SIZE,
    .CSIntervalUnit = kFLEXSPI_CsIntervalUnit1SckCycle,
    .CSInterval = 2,
    .CSHoldTime = 0,
    .CSSetupTime = 4,
    .dataValidTime = 1,
    .columnspace = 3,
    .enableWordAddress = true,
    .AWRSeqIndex = HYPERRAM_CMD_LUT_SEQ_IDX_WRITEDATA,
    .AWRSeqNumber = 1,
    .ARDSeqIndex = HYPERRAM_CMD_LUT_SEQ_IDX_READDATA,
    .ARDSeqNumber = 1,
    .AHBWriteWaitUnit = kFLEXSPI_AhbWriteWaitUnit2AhbCycle,
    .AHBWriteWaitInterval = 0,
    .enableWriteMask = true,
};

uint32_t customLUT[20] = {
        /* Read Data */
        [4 * HYPERRAM_CMD_LUT_SEQ_IDX_READDATA] =
            FLEXSPI_LUT_SEQ(kFLEXSPI_Command_DDR, kFLEXSPI_8PAD, 0xA0, kFLEXSPI_Command_RADDR_DDR, kFLEXSPI_8PAD, 0x18),
        [4 * HYPERRAM_CMD_LUT_SEQ_IDX_READDATA + 1] = FLEXSPI_LUT_SEQ(
            kFLEXSPI_Command_CADDR_DDR, kFLEXSPI_8PAD, 0x10, kFLEXSPI_Command_DUMMY_RWDS_DDR, kFLEXSPI_8PAD, 0x07),
        [4 * HYPERRAM_CMD_LUT_SEQ_IDX_READDATA + 2] =
            FLEXSPI_LUT_SEQ(kFLEXSPI_Command_READ_DDR, kFLEXSPI_8PAD, 0x04, kFLEXSPI_Command_STOP, kFLEXSPI_1PAD, 0x00),

        /* Write Data */
        [4 * HYPERRAM_CMD_LUT_SEQ_IDX_WRITEDATA] =
            FLEXSPI_LUT_SEQ(kFLEXSPI_Command_DDR, kFLEXSPI_8PAD, 0x20, kFLEXSPI_Command_RADDR_DDR, kFLEXSPI_8PAD, 0x18),
        [4 * HYPERRAM_CMD_LUT_SEQ_IDX_WRITEDATA + 1] = FLEXSPI_LUT_SEQ(
            kFLEXSPI_Command_CADDR_DDR, kFLEXSPI_8PAD, 0x10, kFLEXSPI_Command_DUMMY_RWDS_DDR, kFLEXSPI_8PAD, 0x07),
        [4 * HYPERRAM_CMD_LUT_SEQ_IDX_WRITEDATA + 2] = FLEXSPI_LUT_SEQ(
            kFLEXSPI_Command_WRITE_DDR, kFLEXSPI_8PAD, 0x04, kFLEXSPI_Command_STOP, kFLEXSPI_1PAD, 0x00),

        /* Read Register */
        [4 * HYPERRAM_CMD_LUT_SEQ_IDX_READREG] =
            FLEXSPI_LUT_SEQ(kFLEXSPI_Command_DDR, kFLEXSPI_8PAD, 0xE0, kFLEXSPI_Command_RADDR_DDR, kFLEXSPI_8PAD, 0x18),
        [4 * HYPERRAM_CMD_LUT_SEQ_IDX_READREG + 1] = FLEXSPI_LUT_SEQ(
            kFLEXSPI_Command_CADDR_DDR, kFLEXSPI_8PAD, 0x10, kFLEXSPI_Command_DUMMY_RWDS_DDR, kFLEXSPI_8PAD, 0x07),
        [4 * HYPERRAM_CMD_LUT_SEQ_IDX_READREG + 2] =
            FLEXSPI_LUT_SEQ(kFLEXSPI_Command_READ_DDR, kFLEXSPI_8PAD, 0x04, kFLEXSPI_Command_STOP, kFLEXSPI_1PAD, 0x00),

        /* Write Register */
        [4 * HYPERRAM_CMD_LUT_SEQ_IDX_WRITEREG] =
            FLEXSPI_LUT_SEQ(kFLEXSPI_Command_DDR, kFLEXSPI_8PAD, 0x60, kFLEXSPI_Command_RADDR_DDR, kFLEXSPI_8PAD, 0x18),
        [4 * HYPERRAM_CMD_LUT_SEQ_IDX_WRITEREG + 1] = FLEXSPI_LUT_SEQ(
            kFLEXSPI_Command_CADDR_DDR, kFLEXSPI_8PAD, 0x10, kFLEXSPI_Command_DUMMY_RWDS_DDR, kFLEXSPI_8PAD, 0x07),
        [4 * HYPERRAM_CMD_LUT_SEQ_IDX_WRITEREG + 2] = FLEXSPI_LUT_SEQ(
            kFLEXSPI_Command_WRITE_DDR, kFLEXSPI_8PAD, 0x04, kFLEXSPI_Command_STOP, kFLEXSPI_1PAD, 0x00),
};

static uint8_t s_hyper_ram_write_buffer[1024];
static uint8_t s_hyper_ram_read_buffer[1024];

#define DRAM_SIZE 0x80000U


status_t flexspi_hyper_ram_ipcommand_write_data(FLEXSPI_Type *base, uint32_t address, uint32_t *buffer, uint32_t length)
{
    flexspi_transfer_t flashXfer;
    status_t status;

    /* Write data */
    flashXfer.deviceAddress = address;
    flashXfer.port = kFLEXSPI_PortA1;
    flashXfer.cmdType = kFLEXSPI_Write;
    flashXfer.SeqNumber = 1;
    flashXfer.seqIndex = HYPERRAM_CMD_LUT_SEQ_IDX_WRITEDATA;
    flashXfer.data = buffer;
    flashXfer.dataSize = length;

    status = FLEXSPI_TransferBlocking(base, &flashXfer);

    return status;
}

void flexspi_hyper_ram_ahbcommand_write_data(FLEXSPI_Type *base, uint32_t address, uint32_t *buffer, uint32_t length)
{
    uint32_t *startAddr = (uint32_t *)(EXAMPLE_FLEXSPI_AMBA_BASE + address);
    memcpy(startAddr, buffer, length);

    /* Delay some time for tx buffer data finished. */
    uint32_t i = 1000;
    while (i--)
    {
        __NOP();
    }
}

status_t flexspi_hyper_ram_ipcommand_read_data(FLEXSPI_Type *base, uint32_t address, uint32_t *buffer, uint32_t length)
{
    flexspi_transfer_t flashXfer;
    status_t status;

    /* Write data */
    flashXfer.deviceAddress = address;
    flashXfer.port = kFLEXSPI_PortA1;
    flashXfer.cmdType = kFLEXSPI_Read;
    flashXfer.SeqNumber = 1;
    flashXfer.seqIndex = HYPERRAM_CMD_LUT_SEQ_IDX_READDATA;
    flashXfer.data = buffer;
    flashXfer.dataSize = length;

    status = FLEXSPI_TransferBlocking(base, &flashXfer);

    return status;
}

void flexspi_hyper_ram_ahbcommand_read_data(FLEXSPI_Type *base, uint32_t address, uint32_t *buffer, uint32_t length)
{
    uint32_t *startAddr = (uint32_t *)(EXAMPLE_FLEXSPI_AMBA_BASE + address);
    memcpy(buffer, startAddr, length);
}

status_t flexspi_hyper_ram_get_id(FLEXSPI_Type *base, uint32_t *vendorId)
{
    flexspi_transfer_t flashXfer;
    status_t status;
    uint32_t id;

    /* Write data */
    flashXfer.deviceAddress = 0x0U;
    flashXfer.port = kFLEXSPI_PortA1;
    flashXfer.cmdType = kFLEXSPI_Read;
    flashXfer.SeqNumber = 1;
    flashXfer.seqIndex = HYPERRAM_CMD_LUT_SEQ_IDX_READREG;
    flashXfer.data = &id;
    flashXfer.dataSize = 4;

    status = FLEXSPI_TransferBlocking(base, &flashXfer);

    *vendorId = id;// & 0xffffU;

    return status;
}

#define EXAMPLE_SEMC_START_ADDRESS 0x60000000

static void SEMC_SDRAMReadWrite32Bit(void)
{
    uint32_t index;
    uint32_t datalen = (8*1024*1024 - 4)/4;
    uint32_t *sdram  = (uint32_t *)EXAMPLE_SEMC_START_ADDRESS; /* PSRAM start address. */
    bool result      = true;

    for (int i = 0; i < 4; i++) {
        
        sdram  = (uint32_t *)(EXAMPLE_SEMC_START_ADDRESS + i);
        
        PRINTF("\r\n SEMC SDRAM Write 32 bit Start, Start Address 0x%x, Data Length %d !\r\n", sdram, datalen);
        /* Prepare data and write to SDRAM. */
        for (index = 0; index < datalen; index++)
        {
            sdram[index] = index;
        }

        PRINTF("\r\n SEMC SDRAM Read 32 bit Data Start, Start Address 0x%x, Data Length %d !\r\n", sdram, datalen);

        /* Compare the two buffers. */
        for (index = 0; index < datalen; index++)
        {
            if (sdram[index] != index) {
                result = false;
                break;
            }
        }
        if (!result) {
            break;
        }
    }


    if (!result)
    {
        PRINTF("\r\n SEMC SDRAM 32 bit Data Write and Read Compare Failed!\r\n");
    }
    else
    {
        PRINTF("\r\n SEMC SDRAM 32 bit Data Write and Read Compare Succeed!\r\n");
    }
}

static void SEMC_SDRAMReadWrite16Bit(void)
{
    uint32_t index;
    uint32_t datalen = (8*1024*1024 - 2)/2;
    uint16_t *sdram  = (uint16_t *)EXAMPLE_SEMC_START_ADDRESS; /* SDRAM start address. */
    bool result      = true;
    
    for (int i = 0; i < 2; i++) {
        
        sdram  = (uint16_t *)(EXAMPLE_SEMC_START_ADDRESS + i);
        
        PRINTF("\r\n SEMC SDRAM Write 16 bit Start, Start Address 0x%x, Data Length %d !\r\n", sdram, datalen);
        /* Prepare data and write to SDRAM. */
        for (index = 0; index < datalen; index++)
        {
            sdram[index] = index & 0xFFFF;
        }

        PRINTF("\r\n SEMC SDRAM Read 16 bit Data Start, Start Address 0x%x, Data Length %d !\r\n", sdram, datalen);

        /* Compare the two buffers. */
        for (index = 0; index < datalen; index++)
        {
            if (sdram[index] != (index & 0xFFFF)) {
                result = false;
                break;
            }
        }
    }
    if (!result)
    {
        PRINTF("\r\n SEMC SDRAM 16 bit Data Write and Read Compare Failed!\r\n");
    }
    else
    {
        PRINTF("\r\n SEMC SDRAM 16 bit Data Write and Read Compare Succeed!\r\n");
    }
}

static void SEMC_SDRAMReadWrite8Bit(void)
{
    uint32_t index;
    uint32_t datalen = (8*1024*1024);
    uint8_t *sdram   = (uint8_t *)EXAMPLE_SEMC_START_ADDRESS; /* SDRAM start address. */
    bool result      = true;

    PRINTF("\r\n SEMC SDRAM Memory 8 bit Write Start, Start Address 0x%x, Data Length %d !\r\n", sdram, datalen);

    /* Prepare data and write to SDRAM. */
    for (index = 0; index < datalen; index++)
    {
        sdram[index] = index & 0xFF;
    }

    PRINTF("\r\n SEMC SDRAM Read 8 bit Data Start, Start Address 0x%x, Data Length %d !\r\n", sdram, datalen);
    /* Compare the two buffers. */
    for (index = 0; index < datalen; index++)
    {
        if (sdram[index] != (index & 0xFF)) {
            result = false;
            break;
        }
    }

    if (!result)
    {
        PRINTF("\r\n SEMC SDRAM 8 bit Data Write and Read Compare Failed!\r\n");
    }
    else
    {
        PRINTF("\r\n SEMC SDRAM 8 bit Data Write and Read Compare Succeed!\r\n");
    }
}

int hyper_ram_init(void)
{
    uint32_t i      = 0;
    status_t status = kStatus_Fail;
    bool errorFlag  = false;
    uint32_t coreclk = 0;
    uint32_t ahbclk = 0;
    uint32_t hyperbusclk = 0;
    flexspi_config_t config;
    uint32_t vendorID = 0;

    uint32_t t1;
    uint32_t t2;
    uint32_t time;
    uint32_t tmp;
    uint32_t g_ms;
    uint32_t dsize = 32;

    uint8_t *ahb_addr = (uint8_t *)EXAMPLE_FLEXSPI_AMBA_BASE;
    
    //BOARD_ConfigMPU();
    //BOARD_InitBootPins();
    //BOARD_InitBootClocks();
    //BOARD_InitDebugConsole();

    /* Set flexspi root clock to 332MHZ. */
    const clock_usb_pll_config_t g_ccmConfigUsbPll = {.loopDivider = 0U};

    CLOCK_InitUsb1Pll(&g_ccmConfigUsbPll);
    CLOCK_InitUsb1Pfd(kCLOCK_Pfd0, 22);   /* Set PLL3 PFD0 clock 332MHZ. */
    CLOCK_SetMux(kCLOCK_FlexspiMux, 0x3); /* Choose PLL3 PFD0 clock as flexspi source clock. */
    CLOCK_SetDiv(kCLOCK_FlexspiDiv, 1);   /* Divide the clock by 1 to get flexspi root clock 332MHZ */
    
    PRINTF("before disable dcache\r\n");
    
    /* Disable DCache */
    SCB_DisableDCache();

    
    coreclk = CLOCK_GetFreq(kCLOCK_CoreSysClk);
    ahbclk = CLOCK_GetFreq(kCLOCK_IpgClk);
    hyperbusclk = CLOCK_GetFreq(kCLOCK_Usb1PllPfd0Clk);
    PRINTF("coreclk: %d; ahbclk: %d; hyperbusclk: %d\r\n", coreclk, ahbclk, hyperbusclk);
   
    PRINTF("FlexSPI HyperRAM example started!\r\n");


    /* Get FLEXSPI default settings and configure the flexspi. */
    FLEXSPI_GetDefaultConfig(&config);

    /* Init FLEXSPI. */
    config.rxSampleClock = kFLEXSPI_ReadSampleClkExternalInputFromDqsPad;
    config.enableSckBDiffOpt = true;
    config.enableCombination = true;
    config.ahbConfig.enableAHBPrefetch = true;
    config.ahbConfig.enableAHBBufferable = true;
    config.ahbConfig.enableAHBCachable = true;

    FLEXSPI_Init(EXAMPLE_FLEXSPI, &config);

    /* Configure RAM settings according to serial RAM feature. */
    FLEXSPI_SetFlashConfig(EXAMPLE_FLEXSPI, &deviceconfig, kFLEXSPI_PortA1);

    /* Update LUT table. */
    FLEXSPI_UpdateLUT(EXAMPLE_FLEXSPI, 0, customLUT, ARRAY_SIZE(customLUT));

    /* Do software reset. */
    FLEXSPI_SoftwareReset(EXAMPLE_FLEXSPI);

    /* Get vendor ID. */
    status = flexspi_hyper_ram_get_id(EXAMPLE_FLEXSPI, &vendorID);
    if (status != kStatus_Success)
    {
        while (1);
    }
    PRINTF("Vendor ID: 0x%x\r\n", (uint16_t)vendorID);

//#if 1    
//    for (i = 0; i < sizeof(s_hyper_ram_write_buffer); i++)
//    {
//        s_hyper_ram_write_buffer[i] = i;
//    }

//    for (i = 0; i < DRAM_SIZE; i += 2048)
//    {
//        flexspi_hyper_ram_ipcommand_write_data(EXAMPLE_FLEXSPI, i, (uint32_t *)s_hyper_ram_write_buffer,
//                                               sizeof(s_hyper_ram_write_buffer));
//        flexspi_hyper_ram_ipcommand_read_data(EXAMPLE_FLEXSPI, i, (uint32_t *)s_hyper_ram_read_buffer,
//                                              sizeof(s_hyper_ram_read_buffer));

//        if (memcmp(s_hyper_ram_read_buffer, s_hyper_ram_write_buffer, sizeof(s_hyper_ram_write_buffer)) != 0)
//        {
//            PRINTF("IP Command Read/Write data Failure at 0x%x - 0x%x!\r\n", i, i + 2047);
//            return -1;
//        }
//    }

//    PRINTF("IP Command Read/Write data succeed at all address range !\r\n");

//    for (i = sizeof(s_hyper_ram_write_buffer); i > 0; i--)
//    {
//        s_hyper_ram_write_buffer[i] = i;
//    }
//    memset(s_hyper_ram_read_buffer, 0, sizeof(s_hyper_ram_read_buffer));

//    for (i = 0; i < DRAM_SIZE; i += 2048)
//    {
//        flexspi_hyper_ram_ahbcommand_write_data(EXAMPLE_FLEXSPI, i, (uint32_t *)s_hyper_ram_write_buffer,
//                                                sizeof(s_hyper_ram_write_buffer));
//        flexspi_hyper_ram_ahbcommand_read_data(EXAMPLE_FLEXSPI, i, (uint32_t *)s_hyper_ram_read_buffer,
//                                               sizeof(s_hyper_ram_read_buffer));

//        if (memcmp(s_hyper_ram_read_buffer, s_hyper_ram_write_buffer, sizeof(s_hyper_ram_write_buffer)) != 0)
//        {
//            PRINTF("AHB Command Read/Write data Failure at 0x%x - 0x%x!\r\n", i, i + 2047);
//            return -1;
//        }
//    }

//    PRINTF("AHB Command Read/Write data succeed at all address range !\r\n");
//#endif
// 
//    g_msCount = 0;
//    /* HyperRAM write test */
////    SysTick_Config(coreclk / 1000); /* 1ms for one interrupt*/
//  
//    t1 = SysTick->VAL;
//    for (i = 0; i < dsize; i += 4)
//    {
//        *(uint32_t *)(ahb_addr + i) = 0x5a5a5a5a;
//    }
//    __asm("DSB");
//    t2 = SysTick->VAL;
//    g_ms = g_msCount;
//    time = (((uint64_t)(t1 - t2) * 1000000000) / coreclk);
//    PRINTF("##HyperRAM AHB write perf##t1: %d; t2: %d; diff: %d; ns: %d, datasize: %d byte; perf: %dMB/s; g_ms: %d\r\n",
//           t1, t2, t1 - t2, time, dsize, (dsize * 1000)/time, g_ms);
//#if 1
//    g_msCount = 0;
//    /* HyperRAM read test */
//    SysTick_Config(coreclk / 1000); /* 1ms for one interrupt*/
//  
//    t1 = SysTick->VAL;
//    for (i = 0; i < dsize; i += 4)
//    {
//        tmp = *(uint32_t *)(ahb_addr + i);
//    }
//    __asm("DSB");
//    t2 = SysTick->VAL;
//    g_ms = g_msCount;
//    time = (((uint64_t)(t1 - t2) * 1000000000) / coreclk);
//    PRINTF("##HyperRAM AHB read perf###t1: %d; t2: %d; diff: %d; ns: %d, datasize: %d byte; perf: %dMB/s; g_ms: %d\r\n",
//           t1, t2, t1 - t2, time, dsize, (dsize * 1000)/time, g_ms);
//#endif
//    for (i = 0; i < 32; i += 4)
//    {
//      PRINTF("0x%x: 0x%08x 0x%08x 0x%08x 0x%08x\r\n", ahb_addr + i, *(uint32_t *)(ahb_addr + i), *(uint32_t *)(ahb_addr + i + 4)
//                                                          , *(uint32_t *)(ahb_addr + i + 8), *(uint32_t *)(ahb_addr + i + 12));
//    }
    
    /* 32Bit data read and write. */
    SEMC_SDRAMReadWrite32Bit();
    /* 16Bit data read and write. */
    SEMC_SDRAMReadWrite16Bit();
    /* 8Bit data read and write. */
    SEMC_SDRAMReadWrite8Bit();

    PRINTF("\r\n SEMC SDRAM Example End.\r\n");
    SCB_EnableDCache();
}
